#include "gtest/gtest.h"

#include "Matrix.h"

using namespace SG::Algebra;

class MatrixTest : public ::testing::Test
{
  public:
    virtual void SetUp () override
    {
        m = Matrix (2, 3, { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 });
    }

    virtual void TearDown () override
    {}

  protected:
    Matrix m;
};

// 不需要使用基类中的成员
TEST_F (MatrixTest, ConstructTest)
{
    // Test data
    Matrix m1 (2, 3);

    // Test
    ASSERT_EQ (3, m1.GetColSize ());
    ASSERT_EQ (2, m1.GetRowSize ());
}

TEST_F (MatrixTest, ConstructTest2)
{
    // Test data
    Matrix m1 (2, 2, {1,2,3,4});

    // Test
    ASSERT_NEAR (4, m1(1, 1), 1.0e-6);
}

TEST_F (MatrixTest, inverse2X2Test)
{
    // Test data
    Matrix m1 (2, 2, { 1.0, 2.0, 3.0, 4.0 });
    Matrix invTest = inverse2X2 (m1);
    // Target data
    Matrix invTarget (2, 2, { -2.0, 1.0, 1.5, -0.5 });
    // Test
    ASSERT_EQ (invTest, invTarget);
}

TEST_F (MatrixTest, inverse3X3Test)
{
    // Test data
    Matrix m1 (3, 3, { 1.0, 2.0, -1.0, 3.0, 4.0, -2.0, 5.0, -1.0, 1.0 });
    Real_t   detTest;
    Matrix invTest = inverse3X3 (m1, detTest);
    // Target data
    Matrix invTarget (3, 3, { -2.0, 1.0, 0.0, 13.0, -6.0, 1.0, 23.0, -11.0, 2.0 });
    Real_t   detTarget = -1.0;
    // Test
    ASSERT_EQ (invTest, invTarget);
    ASSERT_NEAR (detTest, detTarget, 1e-6);
}

TEST_F (MatrixTest, det3X3Test)
{
    // Test data
    Matrix m1 (3, 3, { 1.0, 2.0, -1.0, 3.0, 4.0, -2.0, 5.0, -1.0, 1.0 });
    Real_t   detTest = det3X3 (m1);
    // Target data
    Real_t detTarget = -1.0;
    // Test
    ASSERT_NEAR (detTest, detTarget, 1e-6);
}

TEST_F (MatrixTest, det4X4Test)
{
    // Test data
    Matrix m1 (4, 4, 
    { 1.0, 0.0, 1.0, 2.0, 
    1.0, 4.0, 3.0, 1.0, 
    1.0, -1.0, 0.0, 0.0, 
    1.0, 2.0, 2.0, 4.0 });
    Real_t   detTest = det4X4 (m1);
    // Target data
    Real_t detTarget = - 5.0;
    // Test
    ASSERT_NEAR (detTest, detTarget, 1e-6);
}


TEST_F (MatrixTest, ParenthesisTest)
{
    // Test data
    auto valueTest = m (1, 2);

    // Target data
    const Real_t valueTarget{ 6.0 };

    // Test
    ASSERT_NEAR (valueTarget, valueTest, 1e-6);
}

TEST_F (MatrixTest, SquareBracketsOperatorTest)
{
    // Test data
    auto valueTest = m[1][1];

    // Target data
    const Real_t valueTarget{ 5.0 };

    // Test
    ASSERT_NEAR (valueTarget, valueTest, 1e-6);
}

TEST_F (MatrixTest, MultiplyByRealTest)
{
    // Test data
    auto mTest = m * 2.0;

    // Target data
    auto mTarget = Matrix (2, 3, { 2.0, 4.0, 6.0, 8.0, 10.0, 12.0 });

    // Test
    ASSERT_TRUE (mTest == mTarget);
}

// 不需要使用基类中的成员
// Square case
TEST_F (MatrixTest, MultiplyByMatrixTest1)
{
    // Input data
    auto a = Matrix (3, 3, { 1.0, 2.0, 3.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0 });

    auto b = Matrix (3, 3, { 0.0, 3.0, 6.0, 1.0, 4.0, 7.0, 2.0, 5.0, 8.0 });
    // Test data
    auto cTest = a * b;

    // Target data
    auto cTarget = Matrix (3, 3, { 8.0, 26.0, 44.0, 14.0, 50.0, 86.0, 23.0, 86.0, 149.0 });

    // Test
    ASSERT_TRUE (cTest == cTarget);
}

// 不需要使用基类中的成员
// 非方矩阵情形1
TEST_F (MatrixTest, MultiplyByMatrixTest2)
{
    // Input data
    auto a = Matrix (2, 4, { 1.0, 2.0, 3.0, 4.0, 3.0, 7.0, 5.0, 8.0 });

    auto b = Matrix (4, 2, { 0.0, 3.0, 6.0, 7.0, 1.0, 4.0, 2.0, -5.3 });
    // Test data
    auto cTest = a * b;

    // Target data
    auto cTarget = Matrix (2, 2, { 23.0000, 7.8000, 63.0000, 35.6000 });

    // Test
    ASSERT_TRUE (cTest == cTarget);
}

// 不需要使用基类中的成员
// 非方矩阵情形2
TEST_F (MatrixTest, MultiplyByMatrixTest3)
{
    // Input data
    auto a = Matrix (2, 3, { 1.0, 2.0, -3.0, 3.0, 7.0, 5.0 });

    auto b = Matrix (3, 4, { 0.0, 3.0, 6.0, 7.0, 6.0, 7.0, -1.1, -5.3, 1.0, 4.0, 2.0, -5.3 });
    // Test data
    auto cTest = a * b;

    // Target data
    auto cTarget = Matrix (2, 4, { 9.0000, 5.0000, -2.2000, 12.3000, 47.0000, 78.0000, 20.3000, -42.6000 });

    // Test
    ASSERT_TRUE (cTest == cTarget);
}

TEST_F (MatrixTest, TransposeTest)
{
    // Test data
    auto transTest = m.Transpose ();
    // output (transTest);

    // Target data
    auto transTarget = Matrix (3, 2, { 1.0, 4.0, 2.0, 5.0, 3.0, 6.0 });

    // Test
    ASSERT_TRUE (transTest == transTarget);
}

TEST_F (MatrixTest, AddEQOperatorTest)
{
    // Input
    auto transTest = Matrix (2, 3, { 1.0, 4.0, -2.0, 5.0, 3.2, -6.1 });
    // Test data
    transTest += m;

    // Target data
    auto transTarget = Matrix (2, 3, { 2.0, 6.0, 1.0, 9.0, 8.2, -0.1 });

    // Test
    ASSERT_TRUE (transTest == transTarget);
}

TEST_F (MatrixTest, AddOperatorTest)
{
    // Input
    auto aTest = Matrix (2, 3, { 1.0, 4.0, -2.0, 5.0, 3.2, -6.1 });
    // Test data
    auto result = aTest + m;

    // Target data
    auto resultTestTarget = Matrix (2, 3, { 2.0, 6.0, 1.0, 9.0, 8.2, -0.1 });

    // Test
    ASSERT_TRUE (result == resultTestTarget);
}

TEST_F (MatrixTest, SubEQOperatorTest)
{
    // Input data
    auto transTest = Matrix (2, 3, { 1.0, 4.0, -2.0, 5.0, 3.2, -6.1 });
    // Test data
    transTest -= m;

    // Target data
    auto transTarget = Matrix (2, 3, { 0.0, 2.0, -5.0, 1.0, -1.8, -12.1 });

    // Test
    ASSERT_TRUE (transTest == transTarget);
}

TEST_F (MatrixTest, SubOperatorTest)
{
    // Input
    auto subTest = Matrix (2, 3, { 1.0, 4.0, -2.0, 5.0, 3.2, -6.1 });
    // Test data
    auto result = subTest - m;

    // Target data
    auto resultTestTarget = Matrix (2, 3, { 0.0, 2.0, -5.0, 1.0, -1.8, -12.1 });

    // Test
    ASSERT_TRUE (result == resultTestTarget);
}

TEST_F (MatrixTest, InverseTest)
{
    // Input data
    auto m1 = Matrix (3, 3, { 1.0, 2.0, 1.0, 3.0, 8.0, 1.0, 0.0, 4.0, 1.0 });
    // Test data
    auto m1Test = m1.Inverse ();

    // Target Data
    auto m1Target = Matrix (3, 3, { 0.4, 0.2, -0.6, -0.3, 0.1, 0.2, 1.2, -0.4, 0.2 });

    // Test
    ASSERT_TRUE (m1Test == m1Target);
}

TEST_F (MatrixTest, InverseTest2)
{
    // Input data
    auto m1 = Matrix (2, 2, { 1.605525893797236e-06,
        1.3881159524806442e-07,
        1.3881159524806442e-07,
        2.0109219412034522e-06 });
    // Test data
    auto m1Test = m1.Inverse ();

    // Target Data
    auto m1Target = Matrix (2, 2, { 626588.444258095, -43252.669200782868, -43252.669200782868, 500270.02609979303});

    // Test
    ASSERT_TRUE (m1Test == m1Target);
}

TEST_F (MatrixTest, BlockTest)
{
    // Input data
    Matrix m (5, 5, {1,  2,  3,  4,  5, 
                     6,  7,  8,  9,  10, 
                     11, 12, 13, 14, 15, 
                     16, 17, 18, 19, 20,
                     21, 22, 23, 24, 25});

    // Test data
    auto mTest = m.Block (1, 0, 2, 4);

    // Target Data
    const Matrix mTarget (2, 4, {6,  7,  8,  9, 
                                 11, 12, 13, 14});

    // Check
    ASSERT_TRUE (mTarget == mTest);
}

TEST_F (MatrixTest, BlockFillTest)
{
    // Input data
    auto m1 = Matrix (3, 3, { 0.4, 0.2, -0.6, -0.3, 0.1, 0.2, 1.2, -0.4, 0.2 });

    // Test data
    Matrix                         m1Test (5, 5);
    const std::vector<std::size_t> rows{ 1, 2, 4 };
    const std::vector<std::size_t> cols{ 0, 2, 4 };
    m1Test.BlockFill (rows, cols, m1);

    // Target Data
    auto m1Target = Matrix (5, 5, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.2, 0.0, -0.6, -0.3, 0.0, 0.1,
                                    0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.2, 0.0, -0.4, 0.0,  0.2 });

    // Test
    ASSERT_TRUE (m1Test == m1Target);
}

TEST_F (MatrixTest, BlockFill2Test)
{
    // Input data
    auto m1 = Matrix (3, 3, { 0.4, 0.2, -0.6, -0.3, 0.1, 0.2, 1.2, -0.4, 0.2 });

    // Test data
    Matrix m1Test (5, 5);
    m1Test.BlockFill (1, 2, m1);

    // Target Data
    auto m1Target = Matrix (5, 5, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  0.0, 0.4, 0.2, -0.6, 0.0, 0.0, -0.3,
                                    0.1, 0.2, 0.0, 0.0, 1.2, -0.4, 0.2, 0.0, 0.0, 0.0,  0.0, 0.0 });

    // Test
    ASSERT_TRUE (m1Test == m1Target);
}

TEST_F (MatrixTest, hstackTest)
{
    // Input data
    auto m = Matrix (5, 5, 
    { 1, 2, 3, 4, 5, 
    6, 7, 8, 9, 10, 
    11, 12, 13, 14, 15, 
    16, 17, 18, 19, 20,
    21, 22, 23, 24, 25 });

    auto n = Matrix (5, 2, 
    { 26, 27, 
    28, 29, 
    30, 31, 
    32, 33, 
    34, 35 });

    auto m1Test = hstack (m, n);

    // Target Data
    auto m1Target = Matrix (5, 7, 
    { 1, 2, 3, 4, 5, 26, 27,
    6, 7, 8, 9, 10, 28, 29,
    11, 12, 13, 14, 15, 30, 31,
    16, 17, 18, 19, 20, 32, 33,
    21, 22, 23, 24, 25, 34, 35 });

    // Test
    ASSERT_TRUE (m1Test == m1Target);
}

TEST_F (MatrixTest, vstackTest)
{
    // Input data
    auto m = Matrix (5, 5, 
    { 1, 2, 3, 4, 5, 
    6, 7, 8, 9, 10, 
    11, 12, 13, 14, 15, 
    16, 17, 18, 19, 20,
    21, 22, 23, 24, 25 });

    auto n = Matrix (2, 5, 
    { 26, 27, 28, 29, 30, 
    31, 32, 33, 34, 35});

    auto m1Test = vstack (m, n);

    // Target Data
    auto m1Target = Matrix (7, 5, 
    { 1, 2, 3, 4, 5, 
    6, 7, 8, 9, 10, 
    11, 12, 13, 14, 15, 
    16, 17, 18, 19, 20,
    21, 22, 23, 24, 25,
    26, 27, 28, 29, 30,
    31, 32, 33, 34, 35 });

    // Test
    ASSERT_TRUE (m1Test == m1Target);
}

TEST_F (MatrixTest, removeTest)
{
    // Input data
    auto m = Matrix (5, 5, 
    { 1, 2, 3, 4, 5, 
    6, 7, 8, 9, 10, 
    11, 12, 13, 14, 15, 
    16, 17, 18, 19, 20,
    21, 22, 23, 24, 25 });

    auto m1Test = remove (m, 2, 0);
    auto m2Test = remove (m, 4, 0);
    auto m3Test = remove (m, 2, 1);
    auto m4Test = remove (m, 4, 1);

    // Target Data
    auto m1Target = Matrix (4, 5, 
    { 1, 2, 3, 4, 5, 
    6, 7, 8, 9, 10, 
    16, 17, 18, 19, 20,
    21, 22, 23, 24, 25 });

    auto m2Target = Matrix (4, 5, 
    { 1, 2, 3, 4, 5, 
    6, 7, 8, 9, 10, 
    11, 12, 13, 14, 15, 
    16, 17, 18, 19, 20 });

    auto m3Target = Matrix (5, 4, 
    { 1, 2, 4, 5, 
    6, 7, 9, 10, 
    11, 12, 14, 15, 
    16, 17, 19, 20,
    21, 22, 24, 25 });

    auto m4Target = Matrix (5, 4, 
    { 1, 2, 3, 4, 
    6, 7, 8, 9, 
    11, 12, 13, 14, 
    16, 17, 18, 19,
    21, 22, 23, 24 });

    // Test
    ASSERT_TRUE (m1Test == m1Target);
    ASSERT_TRUE (m2Test == m2Target);
    ASSERT_TRUE (m3Test == m3Target);
    ASSERT_TRUE (m4Test == m4Target);
}

TEST_F (MatrixTest, orthonormalizeTest)
{
    // Input data
    const Matrix m (3, 3, {4.0, 0.6,  0.0,
                           1.0, 4.4, -0.4,
                           0.5, 0.7,  4.3});

    // Test data
    auto mTest = orthonormalize (m);

    // Target Data
    const Matrix mTarget (3, 3, {9.97637878E-01, -5.28068506E-02, -4.39329027E-02,
                                 4.70303377E-02,  9.91232577E-01, -1.23475206E-01,
                                 5.00680611E-02,  1.21117364E-01,  9.91374689E-01});

    // Check
    ASSERT_TRUE (mTarget == mTest);
}